#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "ethercat.h"
#include <stdio.h>
#include <time.h>

#ifdef _WIN32
#include <windows.h>
#include <stdint.h>
#include <time.h>
#include <stdio.h>

#define CLOCK_MONOTONIC 0

#define SLAVE_POSITION 2         // Position of the slave
#define TARGET_POSITION 100000  // Target position
#define PROFILE_VELOCITY 10000   // Profile velocity
#define PROFILE_ACCELERATION 500 // Profile acceleration
#define PROFILE_DECELERATION 500 // Profile deceleration

#define NSEC_PER_SEC 1000000000
#define EC_TIMEOUTMON 500
#define TIMER_ABSTIME 1

#define CYCTIME_MS 20

// struct sched_param schedp;
char IOmap[4096];
// pthread_t thread1, thread2;
uint64_t diff, maxt, avg, cycle;
struct timeval t1, t2;
int dorun = 0;
int64 toff = 0;
int expectedWKC;
boolean needlf;
volatile int wkc;
boolean inOP;
uint8 currentgroup = 0;

enum
{
   STATE_RESET,
   STATE_INIT,
   STATE_PREREADY,
   STATE_READY,
   STATE_ENABLE,
   STATE_DISABLE
};

int state = STATE_RESET;

int clock_gettime(int clk_id, struct timespec *tp)
{
   if (clk_id != CLOCK_MONOTONIC)
   {
      return -1;
   }

   LARGE_INTEGER frequency;
   LARGE_INTEGER time;

   if (!QueryPerformanceFrequency(&frequency))
   {
      return -1;
   }

   if (!QueryPerformanceCounter(&time))
   {
      return -1;
   }

   tp->tv_sec = (long)(time.QuadPart / frequency.QuadPart);
   tp->tv_nsec = (long)((time.QuadPart % frequency.QuadPart) * 1e9 / frequency.QuadPart);

   return 0;
}

#endif

#ifdef _WIN32
#include <windows.h> // Use Sleep function for Windows system
#else
#include <unistd.h> // Use usleep function for Linux system
#endif

// Delay function
void delay_ms(int milliseconds)
{
#ifdef _WIN32
   Sleep(milliseconds); // Use Sleep for Windows, unit is milliseconds
#else
   usleep(milliseconds * 1000); // Use usleep for Linux, unit is microseconds
#endif
}

void add_timespec(struct timespec *ts, int64 addtime)
{
   long nsec;
   time_t sec;

   nsec = addtime % NSEC_PER_SEC;
   sec = (addtime - nsec) / NSEC_PER_SEC;
   ts->tv_sec += sec;
   ts->tv_nsec += nsec;
   if (ts->tv_nsec > NSEC_PER_SEC)
   {
      nsec = ts->tv_nsec % NSEC_PER_SEC;
      ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
      ts->tv_nsec = nsec;
   }
}

void ec_sync(int64 reftime, int64 cycletime, int64 *offsettime)
{
   static int64 integral = 0;
   int64 delta;
   /* set linux sync point 50us later than DC sync, just as example */
   delta = (reftime - 50000) % cycletime;
   if (delta > (cycletime / 2))
   {
      delta = delta - cycletime;
   }
   if (delta > 0)
   {
      integral++;
   }
   if (delta < 0)
   {
      integral--;
   }
   *offsettime = -(delta / 100) - (integral / 20);
}

static int drive_write8(uint16 slave, uint16 index, uint8 subindex, uint8 value)
{
   int wkc;

   wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

   return wkc;
}

static int drive_write16(uint16 slave, uint16 index, uint8 subindex, uint16 value)
{
   int wkc;

   wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

   return wkc;
}

static int drive_write32(uint16 slave, uint16 index, uint8 subindex, int32 value)
{
   int wkc;

   wkc = ec_SDOwrite(slave, index, subindex, FALSE, sizeof(value), &value, EC_TIMEOUTRXM);

   return wkc;
}

#ifdef _MSC_VER   // Windows MSVC
  #define PACKED
  #pragma pack(push,1)
#else             // GCC/Clang (Linux)
  #define PACKED __attribute__((packed))
#endif
typedef struct PACKED
{
   uint16_t Controlword;
   uint8_t ModeOp;
   uint32_t PPVel;
   int32_t TargetPos;
   uint32_t PPAC;
   uint32_t PPDC;
   // uint8_t padding;
   
} Drive_Outputs;

typedef struct PACKED
{
   uint16_t Statusword;
   uint8_t ModeOp;
   int32_t ActualPos;
   // int32_t ActualVel;
   // int16_t ActualTor;
   // uint8_t padding;
} Drive_Inputs;

#ifdef _MSC_VER
  #pragma pack(pop)
#endif

int drive_setup(uint16 slave)
{
   int wkc = 0;

   printf("Drive %d setup\n",slave);

   wkc += drive_write16(slave, 0x1C12, 0, 0);
   wkc += drive_write16(slave, 0x1C13, 0, 0);

   wkc += drive_write8(slave, 0x1A00, 0, 0);
   wkc += drive_write32(slave, 0x1A00, 1, 0x60410010); // Statusword
   wkc += drive_write32(slave, 0x1A00, 2, 0x60610008); // Modes of operation display
   
   // wkc += drive_write32(slave, 0x1A00, 3, 0x606C0020); // Velocity actual value
   wkc += drive_write32(slave, 0x1A00, 3, 0x60640020); // Position actual value
   // wkc += drive_write32(slave, 0x1A00, 4, 0x60770010); // Torque actual value
   // wkc += drive_write32(slave, 0x1A00, 4, 0x00000008); // padding
   wkc += drive_write8(slave, 0x1A00, 0,3);

   wkc += drive_write8(slave, 0x1600, 0, 0);
   wkc += drive_write32(slave, 0x1600, 1, 0x60400010); // Controlword
   wkc += drive_write32(slave, 0x1600, 2, 0x60600008); // Modes of operation
   wkc += drive_write32(slave, 0x1600, 3, 0x60810020); // PP mode, velocity
   wkc += drive_write32(slave, 0x1600, 4, 0x607A0020); // Target position
   wkc += drive_write32(slave, 0x1600, 5, 0x60830020); // pp ac
   wkc += drive_write32(slave, 0x1600, 6, 0x60840020); // pp dc
   // wkc += drive_write32(slave, 0x1600, 7, 0x00000008); // padding
   wkc += drive_write8(slave, 0x1600, 0, 6);

   wkc += drive_write16(slave, 0x1C12, 1, 0x1600);
   wkc += drive_write8(slave, 0x1C12, 0, 1);

   wkc += drive_write16(slave, 0x1C13, 1, 0x1A00);
   wkc += drive_write8(slave, 0x1C13, 0, 1);

   strncpy(ec_slave[slave].name, "Drive", EC_MAXNAME);

   if (wkc != 21-2)
   {
      printf("Drive %d setup failed\nwkc: %d\n", slave, wkc);
      return -1;
   }
   else
      printf("Drive %d setup succeed.\n", slave);

   return 0;
}

int64_t get_timestamp_clock_gettime()
{
   struct timespec ts;

   if (timespec_get(&ts, TIME_UTC) == TIME_UTC)
   {
      return (int64_t)ts.tv_sec * 1000000000 + ts.tv_nsec;
   }

   return 0;
}

int SV630N(char *ifname)
{
   // Initialize EtherCAT master
   if (ec_init(ifname))
   {
      printf("EtherCAT initialization succeeded.\n");
   }
   else
   {
      printf("EtherCAT initialization failed.\n");
      return -1;
   }

   // Configure slaves
   if (ec_config_init(FALSE) > 0)
   {
      printf("EtherCAT configuration succeeded.\n");
   }
   else
   {
      printf("EtherCAT configuration failed.\n");
      ec_close();
      return -1;
   }

   Drive_Inputs *iptr;
   Drive_Outputs *optr;

   int ht;
   int chk = 2000;
   int64 cycletime;
   struct timespec ts;

   int slave_ix;
   for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
   {
      ec_slavet *slave = &ec_slave[slave_ix];
      printf("PO2SOconfig %d .\n",slave_ix);
      slave->PO2SOconfig = drive_setup;
   }

   /* configure DC options for every DC capable slave found in the list */
   ec_config_map(&IOmap);
   

   // setup dc for devices
   for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
   {
      ec_dcsync0(slave_ix, TRUE, CYCTIME_MS * 1000000U, CYCTIME_MS * 1000000U / 2);
      // ec_dcsync01(slave_ix, TRUE, 4000000U, 8000000U, 20000U);
   }
   ec_configdc();

   printf("Slaves mapped, state to SAFE_OP.\n");
   ec_statecheck(0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE);

   /* read indevidual slave state and store in ec_slave[] */
   ec_readstate();
   for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
   {
      printf("Slave:%d Name:%s Output size:%3dbits Input size:%3dbits State:%2d delay:%d.%d\n",
             slave_ix, ec_slave[slave_ix].name, ec_slave[slave_ix].Obits, ec_slave[slave_ix].Ibits,
             ec_slave[slave_ix].state, (int)ec_slave[slave_ix].pdelay, ec_slave[slave_ix].hasdc);
   }
   expectedWKC = (ec_group[0].outputsWKC * 2) + ec_group[0].inputsWKC;
   printf("Calculated workcounter %d\n", expectedWKC);

   printf("Request operational state for all slaves\n");

   // Wait for all slaves to enter OP state
   ec_statecheck(0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);

   printf("Request operational state for all slaves\n");
   /* activate cyclic process data */
   /* wait for all slaves to reach OP state */
   ec_slave[0].state = EC_STATE_OPERATIONAL;
   /* request OP state for all slaves */
   ec_writestate(0);

   clock_gettime(CLOCK_MONOTONIC, &ts);
   ht = (ts.tv_nsec / 1000000) + 1; /* round to nearest ms */
   ts.tv_nsec = ht * 1000000;
   cycletime = CYCTIME_MS * 1000 * 1000; /* cycletime in ns */

   for (int i = 1; i <= ec_slavecount; i++)
   {
      optr = (Drive_Outputs *)ec_slave[i].outputs;
      if (optr == NULL)
      {
         printf("optr is NULL.\n");
      }
      optr->Controlword = 0;
      optr->ModeOp = 0;
      optr->TargetPos = 0;
      optr->PPVel = 0;
      optr->PPAC = 0;
      optr->PPDC = 0;
   }

   do
   {
      /* wait to cycle start */
      osal_usleep(CYCTIME_MS * 1000);
      if (ec_slave[0].hasdc)
      {
         /* calulate toff to get linux time and DC synced */
         ec_sync(ec_DCtime, cycletime, &toff);
      }
      wkc = ec_receive_processdata(EC_TIMEOUTRET);
      ec_send_processdata();
   } while (chk-- && (wkc != expectedWKC));

   if (wkc == expectedWKC)
   {
      printf("Operational state reached for all slaves.\n");

      int cnt = 0;
      osal_usleep((uint32)(cycletime / 1000));
      toff = 0;
      while (2 > 1)
      {


         osal_usleep((uint32)((cycletime + toff) / 1000));

         if (ec_slave[0].hasdc)
         {
            ec_sync(ec_DCtime, cycletime, &toff);
         }
         wkc = ec_receive_processdata(EC_TIMEOUTRET);


         cnt++;
         for (slave_ix = 1; slave_ix <= ec_slavecount; slave_ix++)
         {

            optr = (Drive_Outputs *)ec_slave[slave_ix].outputs;
            iptr = (Drive_Inputs *)ec_slave[slave_ix].inputs;
            printf(" motor %d data: p %ld,t %ld,s 0x%X,c 0x%X,m %d ", slave_ix, iptr->ActualPos, optr->TargetPos, iptr->Statusword, optr->Controlword,iptr->ModeOp );

            // printf("motor %d data: p %ld, v %ld q %d s 0x%X c 0x%X m %d.\n", slave_ix, iptr->ActualPos, iptr->ActualVel, iptr->ActualTor, iptr->Statusword, optr->Controlword,iptr->ModeOp );
            if (cnt == 2)
            {
               optr->Controlword = 128;
            }
            if (cnt == 4)
            {
               optr->ModeOp = 1;
               optr->TargetPos = iptr->ActualPos;
            }
            if (cnt == 4 * 4)
            {
               optr->Controlword = 6;
               optr->TargetPos = iptr->ActualPos;
            }
            if (cnt == 4 * 8)
            {
               optr->Controlword = 7;
               optr->TargetPos = iptr->ActualPos;
               optr->PPAC = 0x8000;
               optr->PPDC = 0x8000;
            }
            if (cnt == 4 * 12)
            {
               optr->Controlword = 0xF;
            }
            if (cnt == 4 * 16)
            {
               optr->TargetPos = iptr->ActualPos + 300000;
               // optr->TargetPos = 10000;
               optr->PPVel = (uint32_t)(PROFILE_VELOCITY*1.5);
            }
            if (cnt == 4 * 20)
            {
               optr->Controlword = 0x1F;
            }
            // if (cnt == 4 * 36)
            // {
            //    optr->Controlword = 0xF;
            // }            
            // if (cnt == 4 * 40)
            // {
            //    optr->TargetPos = TARGET_POSITION * 2;
            // }   
            // if (cnt == 4 * 40 + 10)
            // {
            //    optr->Controlword = 0x1F;
            // }                   
         }
         printf(" \n");
         ec_send_processdata();
      }
   }
   else /* ECAT进入OP失败 */
   {
      printf("Not all slaves reached operational state.\n");
   }

   ec_readstate();
   for (int i = 1; i <= ec_slavecount; i++)
   {
      printf("Slave %d State=0x%2.2x StatusCode=0x%4.4x : %s\n",
             i, ec_slave[i].state, ec_slave[i].ALstatuscode, ec_ALstatuscode2string(ec_slave[i].ALstatuscode));
   }

   ec_close();
   return 0;
}

char ifbuf[1024];


int main(int argc, char *argv[])
{
   ec_adaptert *adapter = NULL;
   printf(" PDO JiHua \n");

   if (argc > 1)
   {

      strcpy(ifbuf, argv[1]);
      SV630N(ifbuf);
   }
   else
   {
      printf("Usage: slaveinfo ifname [options]\nifname = eth0 for example\nOptions :\n -sdo : print SDO info\n -map : print mapping\n");
      /* Print the list */
      printf("Available adapters\n");
      adapter = ec_find_adapters();
      while (adapter != NULL)
      {
         printf("Description : %s, Device to use for wpcap: %s\n", adapter->desc, adapter->name);
         adapter = adapter->next;
      }
   }

   printf("End program\n");
   return (0);
}